#include "stm32f10x.h"
#include "utype.h"
#include "modbus.h"
#include "uart.h"
#include "Apptimer.h"
#include "MyRtos.h"

// Mudbus function code 
#define ReadHoldReg		0x03		// Read holding register
#define WriteHoldReg		6		// write single holding register
#define WriteHoldRegs		16		// write multiple holding register
#define MY_STATION_ID  0x01				// My Slave  ID
#define MODBUS_RX_BUF_SIZE      256
#define MODBUS_TX_BUF_SIZE      256
#define MODBUS_TMO              200             // 200 ms time out

typedef struct {
  INT8U  StationNo;
  INT8U  Fcn;     // function code = 3
  INT8U  Ref_Hi;
  INT8U  Ref_Lo;
  INT8U  Cnt_Hi;
  INT8U  Cnt_Lo;
  INT16U Crc;
} MODBUS_FCN_3_CMD;    // read holding register command

typedef struct {
  INT8U  StationNo;
  INT8U  Fcn;     // function code = 3
  INT8U  Ref_Hi;
  INT8U  Ref_Lo;
  INT8U  data_Hi;
  INT8U  data_Lo;
  INT16U Crc;
} MODBUS_FCN_6_CMD;    // read holding register command

typedef struct {
  INT8U  StationNo;
  INT8U  Fcn;     // function code = 3
  INT8U  Ref_Hi;
  INT8U  Ref_Lo;
  INT8U  Cnt_Hi;
  INT8U  Cnt_Lo;
  INT8U  Byte_Cnt;
  INT8U  data_Hi;
  INT8U  data_Lo;
  INT16U Crc;
} MODBUS_FCN_16_CMD;    // read holding register command

typedef struct {
  INT8U  StationNo;
  INT8U  Fcn;     // function code = 3
  INT8U  ByteCnt;
  INT8U  Data[256-2]; // data &  CRC
} MODBUS_FCN_3_RSP;    // read holding register reply

typedef struct {
  INT8U  StationNo;
  INT8U  Fcn;     // function code = 3
  INT8U  Ref_Hi;
  INT8U  Ref_Lo;
  INT8U  Cnt_Hi;
  INT8U  Cnt_Lo;
} MODBUS_FCN_6_RSP;    // read holding register reply

typedef struct {
  INT8U  StationNo;
  INT8U  Fcn;     // function code = 3
  INT8U  Ref_Hi;
  INT8U  Ref_Lo;
  INT8U  Cnt_Hi;
  INT8U  Cnt_Lo;
  INT8U  Data[256-2]; // data &  CRC
} MODBUS_FCN_16_RSP;    // read holding register reply


static const USART_InitTypeDef UartParm = {
                 .USART_BaudRate = 19200,
                 .USART_WordLength = USART_WordLength_9b, // data_8+parity
                 .USART_StopBits = USART_StopBits_1,
                 .USART_Parity = USART_Parity_Even,
                 .USART_Mode = 0,
                 .USART_HardwareFlowControl = 0
};

typedef enum {
    ST_WAIT_OPEN_FLAG = 0,
    ST_WAIT_CR_FLAG,
    ST_WAIT_LF_FLAG,
    ST_WAIT_TX_SEND,
    ST_WAIT_TX_DONE
}MODBUS_STATE;

static INT16U ModbusTimer;
static INT8U  Uart1RxBuf[MODBUS_RX_BUF_SIZE];
static INT8U  Uart1TxBuf[MODBUS_TX_BUF_SIZE];
static MODBUS_STATE  ModbusState;


//==========================================================//
//   input:                                                 //
//      pMsgBuf - Pointer to rcv msg.                       //
//      MsgSize - Rcv. msg. size                            //
//      MaxBufSize - Max buffer size for Tx                 //
//   output:                                                //
//      tx msg store in MsgBuf.                             //
//                                                          //
//   return - Parsing result                                //
//        = -1, rcv msg error or not enough  tx buf         //
//        = 0, not for me                                   //
//        >0 , Tx msg size                                  //
//                                                          //
//==========================================================//

INT16U  Reg4xxxx[MAX_4XXXX];

static int BuildReadRegCmd(INT8U *pMsgBuf,int RegNo, int Size)
{
  MODBUS_FCN_3_CMD *p;
  p = (MODBUS_FCN_3_CMD *)pMsgBuf;
  p->StationNo = MY_STATION_ID;
  p->Fcn = ReadHoldReg;
  p->Ref_Hi = RegNo>>8;
  p->Ref_Lo = RegNo;
  p->Cnt_Hi = Size>>8;
  p->Cnt_Lo = Size;
  return 6;
}

static int ModbusSlaveParse(INT8U *pMsgBuf,int MsgSize, int MaxBufSize)
{
	MODBUS_FCN_3_CMD  *pCmd_3;
	MODBUS_FCN_3_RSP  *pRsp_3;
	MODBUS_FCN_6_CMD  *pCmd_6;
	MODBUS_FCN_16_CMD  *pCmd_16;       
	INT16U  *p;
        INT8U *p_byte;
	INT16U  RefNo,WordCnt;
        
  if(pMsgBuf[0] != MY_STATION_ID)	return 0;
  switch  (pMsgBuf[1]) { // parse base on function code
    case ReadHoldReg:
             pCmd_3 = (MODBUS_FCN_3_CMD  *)pMsgBuf;
             pRsp_3 = (MODBUS_FCN_3_RSP  *)pMsgBuf;             
             RefNo = (pCmd_3->Ref_Hi<<8)+pCmd_3->Ref_Lo;
             WordCnt = (pCmd_3->Cnt_Hi<<8)+pCmd_3->Cnt_Lo;
             if((RefNo+WordCnt)>MAX_4XXXX) {
                 MsgSize = 2;
                 pRsp_3->Fcn = ReadHoldReg|0x80;
                 pRsp_3->ByteCnt = 1;
             }
             else {
                 pRsp_3->ByteCnt = WordCnt<<1;
                 if((pRsp_3->ByteCnt+3)>MaxBufSize) return -1;
                 p = (INT16U*)pRsp_3->Data;
                 for(;WordCnt;WordCnt--){
                    *p++ = __REV16(Reg4xxxx[RefNo++]);                   
                 }
                 MsgSize = 3+pRsp_3->ByteCnt;
             }
             return MsgSize;
    case WriteHoldReg:
             pCmd_6 = (MODBUS_FCN_6_CMD  *)pMsgBuf;           
             RefNo = (pCmd_6->Ref_Hi<<8)+pCmd_6->Ref_Lo;
             Reg4xxxx[RefNo] = (pCmd_6->data_Hi<<8)+pCmd_6->data_Lo;
             return 6;
    case WriteHoldRegs:      
             pCmd_16 = (MODBUS_FCN_16_CMD  *)pMsgBuf;           
             RefNo = (pCmd_16->Ref_Hi<<8)+pCmd_16->Ref_Lo;
             WordCnt = (pCmd_16->Cnt_Hi<<8)+pCmd_16->Cnt_Lo;
             p_byte = &pCmd_16->data_Hi;
             p = &Reg4xxxx[RefNo];
             for(;WordCnt;WordCnt--){
               *p++ = (*p_byte<<8)+*(p_byte+1);
               p_byte += 2;
             }
             return 6;      
  }
  return -1;
}

static INT8U a2nibble(INT8U ch)
{
  if(ch>'9') return ch-('A'-10);
  return ch-'0';
}
static INT8U Nibble2Asc(INT8U Nibble)
{
  if(Nibble<10) return '0'+Nibble;
  return Nibble+('A'-10);
}

static INT8U LRC(INT8U *Msg, INT16U Size)
{
  INT8U LRC_Sum;
  LRC_Sum = 0;

  for(;Size;Size--){
    LRC_Sum += *Msg++;
  }
  return LRC_Sum;  
}

static int LRC_Check(INT8U *Msg, INT16U Size)
{
  INT8U LRC_Sum;

  if(Size<=1) return -1;
  LRC_Sum = LRC(Msg,Size);
  if(LRC_Sum) return -1; 
  return 0;
}

static void AsciiBuf2Bin(INT8U *AsciiBuf, INT16U AsciiSize)
{
  INT8U ByteData,*pByteData;
  pByteData = AsciiBuf;
  
  do {
    ByteData = a2nibble(*AsciiBuf++)<<4;
    ByteData |= a2nibble(*AsciiBuf++);
    *pByteData++ = ByteData;
    AsciiSize -= 2;
    
  } while (AsciiSize>0);
}

void BinBuf2Ascii(INT8U *pBin,INT8U *pAsc,int CnvSize)
{
  INT8U ch;
  for(;CnvSize;CnvSize--){
    ch = *pBin++;
    *pAsc++ = Nibble2Asc(ch>>4);
    *pAsc++ = Nibble2Asc(ch&0xf);
  }
}

extern int Uart1PushRxFifo(INT8U *Msg, INT16U Size);

void ModbusTask( void *pvParameters )
{  
  INT16U TxPtr,TxSize,MsgSize,WriteSize,MsgPtr,CnvSize;
  INT8U ch,Lrc, *pTxBuf;
  
  // Command parsing test
  Reg4xxxx[0] = 100;
  
  // Init  
  UartInit(UART_1);
  UartSetLineCoding(UART_1,UartParm);
  ModbusState = ST_WAIT_OPEN_FLAG;
  
  do {
    
    if((ModbusState == ST_WAIT_OPEN_FLAG)||
       (ModbusState == ST_WAIT_CR_FLAG)||
       (ModbusState == ST_WAIT_LF_FLAG)){ 
      do {
         if(UartGetChar(UART_1,&ch)) break;
         if(ChkTimer(ModbusTimer)==0){
            ModbusState = ST_WAIT_OPEN_FLAG;
         }         
         Yield();
      } while (1);
      ModbusTimer = SetTimer(MODBUS_TMO);    
    }
    switch (ModbusState){
      case ST_WAIT_OPEN_FLAG:
        if(ch == ':') {
            ModbusState = ST_WAIT_CR_FLAG;
            MsgSize = 0;            
        }
        break;
      case ST_WAIT_CR_FLAG:
        if(ch == '\r') ModbusState = ST_WAIT_LF_FLAG;
        else {
          if(MsgSize >= MODBUS_RX_BUF_SIZE){
             ModbusState = ST_WAIT_OPEN_FLAG;
             break;
          }
          Uart1RxBuf[MsgSize++] = ch;          
        }      
        break;
      case ST_WAIT_LF_FLAG:
        if(ch == '\n') { // frame end
          AsciiBuf2Bin(Uart1RxBuf,MsgSize);
          MsgSize >>= 1;
          if(LRC_Check(Uart1RxBuf,MsgSize)==0){ // Frame check ok
            MsgSize--;
            MsgSize = ModbusSlaveParse(Uart1RxBuf,MsgSize,MODBUS_RX_BUF_SIZE);
            if(MsgSize > 0) {
                ModbusState = ST_WAIT_TX_SEND;
                Lrc = 0;
                MsgPtr = 0;
                Lrc = 0;
            }
            else {
               ModbusState = ST_WAIT_OPEN_FLAG;
               break;
            }
          }
        }
        else {
               ModbusState = ST_WAIT_OPEN_FLAG;
               break;          
        }
      case ST_WAIT_TX_SEND:
        // prepare part of ascii msg
        if(MsgSize>(MODBUS_TX_BUF_SIZE/2)) CnvSize = MODBUS_TX_BUF_SIZE/2;
        else CnvSize = MsgSize;
        TxSize = CnvSize*2;
        pTxBuf = Uart1TxBuf;
        if(MsgPtr == 0) {  // start of msg
          Uart1TxBuf[0] = ':';
          pTxBuf++;
        }
        BinBuf2Ascii(Uart1RxBuf+MsgPtr,pTxBuf,CnvSize);
        Lrc += LRC(Uart1RxBuf+MsgPtr,CnvSize);
        if(MsgPtr ==0 ) TxSize++;  // include ':'
        MsgPtr += CnvSize;        
        TxPtr = 0;          
        MsgSize -= CnvSize;
        if(MsgSize == 0) { // put LRC
            Lrc = (~Lrc)+1;
            Uart1TxBuf[TxSize++] = Nibble2Asc(Lrc>>4);
            Uart1TxBuf[TxSize++] = Nibble2Asc(Lrc&0xf);
            Uart1TxBuf[TxSize++] = '\r';
            Uart1TxBuf[TxSize++] = '\n';            
        }
        // Put Tx msg in to tx fifo
        do {
          WriteSize = UartWrite(UART_1,Uart1TxBuf+TxPtr,TxSize);
          if(WriteSize){
            TxPtr += WriteSize;
            TxSize -= WriteSize;
          }
          else Yield();
        } while(TxSize);
        if(MsgSize == 0){
          ModbusState = ST_WAIT_TX_DONE;
        }
        break;
      case ST_WAIT_TX_DONE:
        if(UartTxDone(UART_1)){
          ModbusState = ST_WAIT_OPEN_FLAG;
        }
        else Yield();
        break;
    }
  } while (1);
  
}
